#!/usr/bin/env python3
"""
无人机导航系统启动文件
启动所有必要的节点和配置
"""

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, LogInfo, GroupAction
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    """生成launch描述"""
    
    # 获取包路径
    pkg_drone_navigation = get_package_share_directory('drone_navigation')
    
    # 参数文件路径
    params_file = os.path.join(pkg_drone_navigation, 'config', 'navigation_params.yaml')
    
    # 声明launch参数
    declare_params_file = DeclareLaunchArgument(
        'params_file',
        default_value=params_file,
        description='导航系统参数文件路径'
    )
    
    declare_use_sim_time = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='是否使用仿真时间'
    )
    
    declare_enable_serial = DeclareLaunchArgument(
        'enable_serial',
        default_value='false',
        description='是否启用串口通信'
    )
    
    declare_debug_mode = DeclareLaunchArgument(
        'debug_mode',
        default_value='false',
        description='是否启用调试模式'
    )
    
    declare_log_level = DeclareLaunchArgument(
        'log_level',
        default_value='info',
        description='日志级别 (debug, info, warn, error)'
    )
    
    # 获取launch配置
    params_file_config = LaunchConfiguration('params_file')
    use_sim_time = LaunchConfiguration('use_sim_time')
    enable_serial = LaunchConfiguration('enable_serial')
    debug_mode = LaunchConfiguration('debug_mode')
    log_level = LaunchConfiguration('log_level')
    
    # 导航管理器节点
    navigation_manager_node = Node(
        package='drone_navigation',
        executable='navigation_manager',
        name='navigation_manager',
        output='screen',
        parameters=[
            params_file_config,
            {'use_sim_time': use_sim_time}
        ],
        arguments=['--ros-args', '--log-level', log_level],
        remappings=[
            # 重映射话题以连接point_lio
            ('/current_pose', '/Odometry'),  # point_lio里程计输出
        ]
    )
    
    # 障碍物检测器节点
    obstacle_detector_node = Node(
        package='drone_navigation',
        executable='obstacle_detector',
        name='obstacle_detector',
        output='screen',
        parameters=[
            params_file_config,
            {'use_sim_time': use_sim_time}
        ],
        arguments=['--ros-args', '--log-level', log_level],
        remappings=[
            ('/current_pose', '/Odometry'),  # point_lio里程计输出
        ]
    )
    
    # 路径规划器节点
    drone_navigator_node = Node(
        package='drone_navigation',
        executable='drone_navigator',
        name='drone_navigator',
        output='screen',
        parameters=[
            params_file_config,
            {'use_sim_time': use_sim_time}
        ],
        arguments=['--ros-args', '--log-level', log_level],
        remappings=[
            ('/current_pose', '/Odometry'),  # point_lio里程计输出
        ]
    )
    
    # 速度控制器节点
    velocity_controller_node = Node(
        package='drone_navigation',
        executable='velocity_controller',
        name='velocity_controller',
        output='screen',
        parameters=[
            params_file_config,
            {
                'use_sim_time': use_sim_time,
                'enable_serial': enable_serial
            }
        ],
        arguments=['--ros-args', '--log-level', log_level]
    )
    
    # TF静态变换发布器 - 连接不同坐标系
    tf_static_transforms = [
        # base_link到livox_frame的变换（根据实际安装位置调整）
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='base_to_livox_tf',
            arguments=[
                '0', '0', '0.1',  # x, y, z偏移
                '0', '0', '0', '1',  # 四元数 (无旋转)
                'base_link', 'livox_frame'
            ]
        ),
        
        # map到odom的变换（如果point_lio没有提供）
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='map_to_odom_tf',
            arguments=[
                '0', '0', '0',
                '0', '0', '0', '1',
                'map', 'camera_init'  # point_lio使用camera_init作为固定frame
            ]
        )
    ]
    
    # 调试节点组（仅在调试模式下启动）
    debug_nodes = GroupAction(
        condition=IfCondition(debug_mode),
        actions=[
            LogInfo(msg="启动调试模式..."),
            
            # RViz可视化
            Node(
                package='rviz2',
                executable='rviz2',
                name='rviz2',
                output='screen',
                arguments=['-d', os.path.join(pkg_drone_navigation, 'config', 'navigation_rviz.rviz')],
                parameters=[{'use_sim_time': use_sim_time}],
                condition=IfCondition(debug_mode)
            ),
            
            # 话题监控节点
            Node(
                package='rqt_topic',
                executable='rqt_topic',
                name='topic_monitor',
                condition=IfCondition(debug_mode)
            )
        ]
    )
    
    # 系统信息打印
    system_info = LogInfo(msg=[
        '\n=== 无人机导航系统启动 ===\n',
        '硬件平台: Jetson Orin Nano 8GB\n',
        '传感器: Livox Mid360\n',
        '定位系统: point_lio\n',
        '导航框架: ROS2 Humble + Navigation2\n',
        '串口通信: ', enable_serial, '\n',
        '调试模式: ', debug_mode, '\n',
        '参数文件: ', params_file_config, '\n',
        '================================\n'
    ])
    
    # 构建launch描述
    ld = LaunchDescription()
    
    # 添加参数声明
    ld.add_action(declare_params_file)
    ld.add_action(declare_use_sim_time)
    ld.add_action(declare_enable_serial)
    ld.add_action(declare_debug_mode)
    ld.add_action(declare_log_level)
    
    # 添加系统信息
    ld.add_action(system_info)
    
    # 添加TF变换
    for tf_node in tf_static_transforms:
        ld.add_action(tf_node)
    
    # 添加核心导航节点
    ld.add_action(navigation_manager_node)
    ld.add_action(obstacle_detector_node)
    ld.add_action(drone_navigator_node)
    ld.add_action(velocity_controller_node)
    
    # 添加调试节点组
    ld.add_action(debug_nodes)
    
    return ld 